The main contribution of this paper is an invariant extended Kalman filter(EKF) for visual inertial navigation systems (VINS). It is demonstrated thatthe conventional EKF based VINS is not invariant under the stochasticunobservable transformation, associated with translations and a rotation aboutthe gravitational direction. This can lead to inconsistent state estimates asthe estimator does not obey a fundamental property of the physical system. Toaddress this issue, we use a novel uncertainty representation to derive a RightInvariant error extended Kalman filter (RIEKF-VINS) that preserves thisinvariance property. RIEKF-VINS is then adapted to the multistate constraintKalman filter framework to obtain a consistent state estimator. Both MonteCarlo simulations and real-world experiments are used to validate the proposedmethod.
展开▼